/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.19
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *
 *                                                                         *
 ***************************************************************************/
#include "chainSolverTest.hpp"
#include <kdl/frames_io.hpp>
#include <kdl/framevel_io.hpp>
#include <kdl/frameacc_io.hpp>
#include <kdl/kinfam_io.hpp>
#include <iostream>
#include <sys/time.h>
#include <time.h>

#define IF_DEBUG false
#define NSEC_PER_SEC    1000000000
#define TEST_NUM 10000

using namespace KDL;

ChainSolverTest::ChainSolverTest()
{
    this->puma = Puma560();
    this->kuka = KukaLWR_DHnew();
    this->jnPuma=this->puma.getNrOfJoints();
    this->jnKuka=this->kuka.getNrOfJoints();
    this->jointPuma.resize(this->jnPuma);
    this->jointKuka.resize(this->jnKuka);

}

ChainSolverTest::~ChainSolverTest()
{

}

void ChainSolverTest::printTimeNano(struct timespec& t1,struct timespec& t2)
{
    if(t1.tv_nsec>t2.tv_nsec)
    {
        std::cout << t2.tv_sec-t1.tv_sec-1 << "second," <<
                     NSEC_PER_SEC-t1.tv_nsec+t2.tv_nsec  <<"nano second"<< std::endl;
    }
    else
    {
        std::cout << t2.tv_sec-t1.tv_sec << "second," <<
                     t2.tv_nsec - t1.tv_nsec << "nano second" << std::endl;
    }
}

void ChainSolverTest::chainIkPosSolver(const JntArray& joint,
                                          ChainFkSolverPos& fkPosSolver,
                                          JntArray& jointInit,
                                          ChainIkSolverPos& ikPosSolver,
                                          JntArray& jointOut)
{
    int jn = joint.rows();
    Frame fp,fk;
    struct timespec t1,t2;
    if(IF_DEBUG){
        std::cout<<"jointArray:"<<std::endl;
        for(int i=0; i<jn; i++)
        {
            std::cout << joint(i) << " ";
        }
        std::cout << std::endl;
    }
    assert(fkPosSolver.JntToCart(joint, fp)==0);
    if(IF_DEBUG){
        std::cout<<"frame of jointArray:"<<std::endl;
        std::cout << fp << std::endl;
    }
    assert(ikPosSolver.CartToJnt(jointInit,fp,jointOut)==0);
    if(IF_DEBUG){
        std::cout<<"inverse soulution:"<<std::endl;
        for(int i=0; i<jn; i++)
        {
            std::cout << jointOut(i) << " ";
        }
        std::cout << std::endl;
    }
    assert(fkPosSolver.JntToCart(jointOut, fk)==0);
    if(IF_DEBUG){
        std::cout<<"frame of inverse output:"<<std::endl;
        std::cout << fk << std::endl;
    }
//    assert(fk==fp);
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int j=0; j<TEST_NUM; j++)
    {
        assert(ikPosSolver.CartToJnt(jointInit, fp, jointOut)==0);
//        assert(this->jointPuma==jointPumaOut);
     }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);
}

void ChainSolverTest::chainFkPosSolverTest()
{
    ChainFkSolverPos_recursive pumaFkPosSolver = ChainFkSolverPos_recursive(this->puma);
    ChainFkSolverPos_recursive kukaFkPosSolver = ChainFkSolverPos_recursive(this->kuka);
//    ChainFkSolverVel_recursive pumaFkVelSolver = ChainFkSolverVel_recursive(puma);
//    ChainFkSolverVel_recursive kukaFkVelSolver = ChainFkSolverVel_recursive(kuka);
    Frame fp,fp1,fk,fk1;
    struct timespec t1,t2;

/********model--puma********/
     for(int i=0; i<this->jnPuma; i++)
     {
         random(this->jointPuma(i));
      }
    assert(pumaFkPosSolver.JntToCart(this->jointPuma,fp)==0);
    if(IF_DEBUG){
        std::cout<<"forword soulution of Puma560:"<<std::endl;
        std::cout<<fp<<std::endl;
    }
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int j=0; j<TEST_NUM; j++)
    {
       fp1 = fp;
       assert(pumaFkPosSolver.JntToCart(this->jointPuma,fp)==0);
       assert(fp==fp1);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);

///********model--kuka********/
//    for(int i=0; i<this->jnKuka; i++)
//    {
//       random(this->jointKuka(i));
//    }
//    assert(kukaFkPosSolver.JntToCart(this->jointKuka,fk)==0);
//    if(IF_DEBUG){
//        std::cout<<"forword soulution of kukaLWR_DHnew:"<<std::endl;
//        std::cout<<fk<<std::endl;
//    }
//    clock_gettime(CLOCK_REALTIME, &t1);
//    for(int j=0; j<TEST_NUM; j++)
//    {
//        fk1 = fk;
//        assert(kukaFkPosSolver.JntToCart(this->jointKuka,fk)==0);
//        assert(fk==fk1);
//     }
//    clock_gettime(CLOCK_REALTIME, &t2);
//    this->printTimeNano(t1, t2);

}

void ChainSolverTest::chainIkPosSolverTest_LMA()
{
    JntArray jointPumaInit = JntArray(this->jnPuma);
    JntArray jointPumaOut = JntArray(this->jnPuma);
    JntArray jointKukaInit =  JntArray(this->jnKuka);
    JntArray jointKukaOut = JntArray(this->jnKuka);
    ChainFkSolverPos_recursive pumaFkPosSolver = ChainFkSolverPos_recursive(this->puma);
    ChainFkSolverPos_recursive kukaFkPosSolver = ChainFkSolverPos_recursive(this->kuka);
    ChainIkSolverPos_LMA pumaIkPosSolver_LMA = ChainIkSolverPos_LMA(this->puma);
    ChainIkSolverPos_LMA kukaIkPosSolver_LMA = ChainIkSolverPos_LMA(this->kuka);

//********LMA********//

//********model--puma********//
    for(int i=0; i<jnPuma; i++)
    {
        random(this->jointPuma(i));
    }
    std::cout << "inverse solution of Puma560 (LMA) :" << std::endl;
    this->chainIkPosSolver(this->jointPuma, pumaFkPosSolver, jointPumaInit, pumaIkPosSolver_LMA, jointPumaOut);

////********model--kuka********//
//    for(int i=0; i<this->jnKuka; i++)
//    {
//        random(this->jointKuka(i));
//    }
//    std::cout << "inverse solution of kukaLWR_DHnew (LMA) :" << std::endl;
//    this->chainIkPosSolver(this->jointKuka, kukaFkPosSolver, jointKukaInit, kukaIkPosSolver_LMA, jointKukaOut);

}

void ChainSolverTest::chainIkPosSolverTest_NR()
{
    JntArray jointPumaInit = JntArray(this->jnPuma);
    JntArray jointPumaOut = JntArray(this->jnPuma);
    JntArray jointKukaInit =  JntArray(this->jnKuka);
    JntArray jointKukaOut = JntArray(this->jnKuka);
    ChainFkSolverPos_recursive pumaFkPosSolver = ChainFkSolverPos_recursive(this->puma);
    ChainFkSolverPos_recursive kukaFkPosSolver = ChainFkSolverPos_recursive(this->kuka);
    ChainIkSolverVel_pinv pumaIkVelSolver = ChainIkSolverVel_pinv(this->puma);
    ChainIkSolverPos_NR pumaIkPosSolver_NR = ChainIkSolverPos_NR(this->puma,pumaFkPosSolver,pumaIkVelSolver);
    ChainIkSolverVel_pinv kukaIkVelSolver = ChainIkSolverVel_pinv(this->kuka);
    ChainIkSolverPos_NR kukaIkPosSolver_NR = ChainIkSolverPos_NR(this->kuka,kukaFkPosSolver,kukaIkVelSolver);
//    ChainIkSolverVel_pinv_givens pumaIkVelSolver_givens =ChainIkSolverVel_pinv_givens(this->puma);
//    ChainIkSolverPos_NR pumaIkPosSolver_NR_givens = ChainIkSolverPos_NR(this->puma,pumaFkPosSolver,pumaIkVelSolver_givens,1000);
//    ChainIkSolverVel_pinv_givens kukaIkVelSolver_givens =ChainIkSolverVel_pinv_givens(this->kuka);
//    ChainIkSolverPos_NR kukaIkPosSolver_NR_givens = ChainIkSolverPos_NR(this->kuka,kukaFkPosSolver,kukaIkVelSolver_givens,1000);

    //********NR********//

    //********model-Puma360********//
        for(int i=0; i<this->jnPuma; i++)
        {
            random(this->jointPuma(i));
        }
        std::cout<<"inverse soulution of Puma560 (NR) :"<<std::endl;
        this->chainIkPosSolver(this->jointPuma, pumaFkPosSolver, jointPumaInit, pumaIkPosSolver_NR, jointPumaOut);

//    //********model-KukaLWR_DHnew:********//
//        for(int i=0; i<this->jnKuka; i++)
//        {
//            random(this->jointKuka(i));
//        }
//        std::cout<<"inverse soulution of KukaLWR_DHnew (NR) :"<<std::endl;
//        this->chainIkPosSolver(this->jointKuka, kukaFkPosSolver, jointKukaInit, kukaIkPosSolver_NR, jointKukaOut);

////********NR_givens********//

////********model-Puma360********//
//        for(int i=0; i<this->jnPuma; i++)
//        {
//            random(this->jointPuma(i));
//        }
//        double tmp;
//        for(int i=0; i<this->jnPuma; i++)
//        {
//            random(tmp);
//            jointPumaInit(i) = this->jointPuma(i) + 0.1*tmp;
//        }
//        std::cout<<"inverse soulution of Puma560 (NR_givens) :"<<std::endl;
//        this->chainIkPosSolver(this->jointPuma, pumaFkPosSolver, jointPumaInit, pumaIkPosSolver_NR_givens, jointPumaOut);

////********model-KukaLWR_DHnew:********//
//    for(int i=0; i<this->jnKuka; i++)
//    {
//        random(this->jointKuka(i));
//    }
//    std::cout<<"inverse soulution of KukaLWR_DHnew (NR_givens) :"<<std::endl;
//    this->chainIkPosSolver(this->jointKuka, kukaFkPosSolver, jointKukaInit, kukaIkPosSolver_NR_givens, jointKukaOut);

}

void ChainSolverTest::chainIkPosSolverTest_NR_JL()
{
    JntArray jointPumaInit(this->jnPuma);
    JntArray jointPumaOut(this->jnPuma);
    ChainFkSolverPos_recursive pumaFkPosSolver = ChainFkSolverPos_recursive(this->puma);
    ChainIkSolverVel_pinv pumaIkVelSolver = ChainIkSolverVel_pinv(this->puma);
    JntArray jointMin(this->jnPuma);
    JntArray jointMax(this->jnPuma);
    for(int i=0; i<this->jnPuma; i++)
    {
        jointMin(i) = -2*M_PI;
        jointMax(i) = 2*M_PI;
    }
    ChainIkSolverPos_NR_JL pumaIkPosSolver_NR_JL = ChainIkSolverPos_NR_JL(this->puma,jointMin, jointMax, pumaFkPosSolver,pumaIkVelSolver);
    for(int i=0; i<this->jnPuma; i++)
    {
        random(this->jointPuma(i));
    }
    std::cout<<"inverse soulution of Puma560 (NR_JL) :"<<std::endl;
    this->chainIkPosSolver(this->jointPuma, pumaFkPosSolver, jointPumaInit, pumaIkPosSolver_NR_JL, jointPumaOut);

}

void ChainSolverTest::chainVelSolver(const Chain& chain,
                                         ChainFkSolverVel& fkSolverVel,
                                         ChainIkSolverVel& ikSolverVel)
{
    int jn = chain.getNrOfJoints();
    JntArray q(jn);
    JntArray qdot(jn);
    struct timespec t1,t2;
    for(int i=0; i<jn; i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q,qdot);
    JntArray qdot_solved(jn);
    FrameVel cart;
    assert(fkSolverVel.JntToCart(qvel, cart)==0);
    std::cout << "forward:" <<std::endl;
    if(IF_DEBUG){
        std::cout << "forward solution of velocity:" <<std::endl;
        std::cout << "dot of joint:" <<std::endl;
        for(int i=0; i<jn; i++)
        {
            std::cout << qvel.qdot(i) <<" ";
        }
        std::cout << std::endl;
        std::cout << "frame of velocity:" <<std::endl;
        std::cout << cart <<std::endl;
    }
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TEST_NUM; i++)
    {
        assert(fkSolverVel.JntToCart(qvel, cart)==0);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);

    assert(ikSolverVel.CartToJnt(qvel.q,cart.deriv(),qdot_solved)>=0);
    qvel.qdot = qdot_solved;
    FrameVel cart_solved;
    assert(fkSolverVel.JntToCart(qvel, cart_solved)==0);
    assert(Equal(cart.deriv(), cart_solved.deriv(), 1e-5));
    std::cout << "inverse:" <<std::endl;
    if(IF_DEBUG){
        std::cout << "inverse solution of velocity:" <<std::endl;
        std::cout << "dot of joint:" <<std::endl;
        for(int i=0; i<jn; i++)
        {
            std::cout << qdot_solved(i) <<" ";
        }
        std::cout << std::endl;
        std::cout << "frame of velocity:" <<std::endl;
        std::cout << cart_solved <<std::endl;
    }
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TEST_NUM; i++)
    {
        assert(ikSolverVel.CartToJnt(qvel.q,cart.deriv(),qdot_solved)>=0);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);

}

void ChainSolverTest::chainFkandIkVelTest()
{
    ChainFkSolverVel_recursive pumaFkVelSolver(this->puma);
    ChainIkSolverVel_pinv pumaIkVelSolver_pinv(this->puma);
    ChainIkSolverVel_pinv_givens pumaIkVelSolver_pinv_givens(this->puma);
    std::cout << "pinv of Puma360:" <<std::endl;
    this->chainVelSolver(this->puma, pumaFkVelSolver, pumaIkVelSolver_pinv);
    std::cout << "pinv_givens of Puma360:" <<std::endl;
    this->chainVelSolver(this->puma, pumaFkVelSolver, pumaIkVelSolver_pinv_givens);

//    ChainFkSolverVel_recursive kukaFkVelSolver(this->kuka);
//    ChainIkSolverVel_pinv kukaIkVelSolver_pinv(this->kuka);
//    ChainIkSolverVel_pinv_givens kukaIkVelSolver_pinv_givens(this->kuka);
//    std::cout << "pinv of kukaLWR_DHnew:" <<std::endl;
//    this->chainVelSolver(this->kuka, kukaFkVelSolver, kukaIkVelSolver_pinv);
//    std::cout << "pinv_givens of kukaLWR_DHnew:" <<std::endl;
//    this->chainVelSolver(this->kuka, kukaFkVelSolver, kukaIkVelSolver_pinv_givens);

}

void ChainSolverTest::chainIkVelTest_nso_wdls()
{
    ChainFkSolverVel_recursive pumaFkVelSolver(this->puma);
    ChainIkSolverVel_pinv_nso pumaIkVelSolver_pinv_nso(this->puma);
    ChainIkSolverVel_wdls pumaIkVelSolver_wdls(this->puma);
    JntArray q(this->jnPuma);
    JntArray qdot(this->jnPuma);
    struct timespec t1,t2;
    for(int i=0; i<this->jnPuma; i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q,qdot);
    JntArray qdot_solved(this->jnPuma);
    FrameVel cart;
    assert(pumaFkVelSolver.JntToCart(qvel, cart)==0);
//    std::cout << "pinv_nso of Puma560:" <<std::endl;
//    std::cout << pumaIkVelSolver_pinv_nso.CartToJnt(q, cart.deriv(), qdot_solved) <<std::endl;
//    assert(pumaIkVelSolver_pinv_nso.CartToJnt(q, cart.deriv(), qdot_solved)==0);

    std::cout << "wdls of Puma560:" <<std::endl;
    assert(pumaIkVelSolver_wdls.CartToJnt(q, cart.deriv(), qdot_solved)==0);
    if(IF_DEBUG){
        std::cout << "inverse solution of velocity:" <<std::endl;
        std::cout << "dot of joint:" <<std::endl;
        std::cout << qdot_solved <<std::endl;
    }
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TEST_NUM; i++)
    {
        assert(pumaIkVelSolver_wdls.CartToJnt(q, cart.deriv(), qdot_solved)==0);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);

}

void ChainSolverTest::chainAccSolver(const Chain& chain, ChainFkSolverAcc& fkSolverAcc, ChainIkSolverAcc& ikSolverAcc)
{
    int jn = chain.getNrOfJoints();
    JntArray q(jn);
    JntArray qdot(jn);
    JntArray qdotdot(jn);
    struct timespec t1,t2;
    for(int i=0; i<jn; i++)
    {
        random(q(i));
        random(qdot(i));
        random(qdotdot(i));
    }
    JntArrayAcc qacc(q,qdot,qdotdot);
    JntArray qdotdot_solved(jn);
    FrameAcc cart;
    assert(fkSolverAcc.JntToCart(qacc, cart)==0);
    if(IF_DEBUG){
        std::cout << "forward solution of acc:" <<std::endl;
        std::cout << "dot of joint:" <<std::endl;
        for(int i=0; i<jn; i++)
        {
            std::cout << qacc.qdotdot(i) <<" ";
        }
        std::cout << std::endl;
        std::cout << "frame of acc:" <<std::endl;
        std::cout << cart <<std::endl;
    }
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TEST_NUM; i++)
    {
        assert(fkSolverAcc.JntToCart(qacc, cart)==0);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);

}

void ChainSolverTest::chainFkandIkAccTest()
{
//    ChainFkSolverAcc pumaFkAccSolver(this->puma);
//    ChainIkSolverAcc pumaIkAccSolver(this->puma);
//    std::cout << "acc solver of Puma360:" <<std::endl;
//    this->chainAccSolver(this->puma, pumaFkAccSolver, pumaIkAccSolver);

}

void ChainSolverTest::chainIdSolverTest()
{
    int jn = this->puma.getNrOfJoints();
    JntArray q(jn);
    JntArray qdot(jn);
    JntArray qdotdot(jn);
    struct timespec t1,t2;
    for(int i=0; i<jn; i++)
    {
        random(q(i));
        random(qdot(i));
        random(qdotdot(i));
    }
    JntArray tor(jn);
    ChainIdSolver_RNE idSolver_RNE(this->puma, Vector(0.0,0.0,-9.81));
    Wrenches f(this->puma.getNrOfSegments());
    std::cout << "idsolver RNE:" << std::endl;
    assert(idSolver_RNE.CartToJnt(q, qdot, qdotdot, f, tor)==0);
    if(IF_DEBUG)
    {
        std::cout << "torque of RNE:" << std::endl;
        std::cout << tor << std::endl;
    }
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TEST_NUM; i++)
    {
        assert(idSolver_RNE.CartToJnt(q, qdot, qdotdot, f, tor)==0);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    this->printTimeNano(t1, t2);

}
